/**
 * Implement the 3D predicates for primitive intersections. 
 * @author Songgang Xu (sxu at tamu dot edu)
 * @file predicate_3d.h
 */


#ifndef __PREDICATE_3D_H
#define __PREDICATE_3D_H

#include "predicates.h"

extern const double tiny_value ;
/**
 * Decide the sign of det of 4 x 4. the vector are view as column vector. 
 * This is not robust if the computation is not exact.
 */
template <typename Value>
inline Value orient_3d (const Vector<Value> & a, const Vector<Value> & b,  const Vector<Value> & c, const Vector<Value> & d)
{
	return (d - a) * ((b - a) ^ (c - a)) ;
}

/**
 * This function predicate the co-planar convex polygon - segment intersection. 
 * We may use http://gpwiki.org/index.php/Polygon_Collision. 
 * Here we check every intersection of edges. 
 */
template <typename Value>
inline bool predicate_polygon_convex_segment_coplanar (const Polygon <Value> & _pol1, const Segment <Value> & _seg)
{
	Vector<Value> normal = _pol1.calculate_normal();
	unsigned x, y; 
    select_map_axis(normal, x, y);


	unsigned size1 = _pol1.size();

	Polygon <Value> _pol1_2d;

	for (unsigned i = 0; i < size1; ++i)
	{
		_pol1_2d.append_vertex (Vector<Value> (_pol1[i][x], _pol1[i][y]) );
	}


	Segment <Value> _seg_2d; 
	_seg_2d.p0 = Vector <Value> (_seg.p0[x], _seg.p0[y]);
	_seg_2d.p1 = Vector <Value> (_seg.p1[x], _seg.p1[y]);

	std::cout<<_pol1_2d<<std::endl;
	std::cout<<_seg_2d<<std::endl;
	if ((_seg_2d.p0 - _seg_2d.p1).square_module() < tiny_value)
	{
		return  predicate_pol_convex_point (_pol1, _seg_2d.p0);
	}
	bool flag1 = predicate_pol_convex_point (_pol1, _seg_2d.p0);
	bool flag2 = predicate_pol_convex_point (_pol1, _seg_2d.p1);
	if (flag1 || flag2) return true; 

	for (unsigned  i = 0, j = size1 - 1; i < size1; j = i++)
	{
		if (predicate_seg_seg_2d(Segment<Value> (_pol1_2d[i], _pol1_2d[j]), _seg_2d) ) return true; 
	}
	return false; 
	
	//return true; 
}

/**
 * Predicate the interseciton of 2D segments
 */
template<typename Value>
bool predicate_seg_seg_2d (const Segment<Value> & seg1, const Segment<Value> & seg2)
{
	Value d0 = orient_2d(seg1.p0, seg1.p1, seg2.p0);
	Value d1 = orient_2d(seg1.p0, seg1.p1, seg2.p1);

	Value d2 = orient_2d(seg2.p0, seg2.p1, seg1.p0);
	Value d3 = orient_2d(seg2.p0, seg2.p1, seg1.p1);

	if (d0 == 0 && d1 == 0)
	{
		Segment<Value> s1 = seg1 ;
		Segment<Value> s2 = seg2 ;
		s1.order(); s2.order ();
		if (s1.p0 < s2.p1 || s2.p0 < s1.p1)  return false; else return false;
	}
	return (d0 * d1 <=0 && d2 * d3 <=0);
}


/**
 * This function predicate the co-planar convex polygon - polygon intersection.
 * We may use http://gpwiki.org/index.php/Polygon_Collision. 
 * Here we check every intersection of edges. 
 */
template <typename Value>
inline bool predicate_polygon_polygon_convex_coplanar (const Polygon <Value> & _pol1, const Polygon <Value> & _pol2)
{
	
	Vector<Value> normal = _pol1.calculate_normal();
	unsigned x, y; 
    select_map_axis(normal, x, y);


	unsigned size1 = _pol1.size();
	unsigned size2 = _pol2.size(); 

	Polygon <Value> _pol1_2d, _pol2_2d;

	for (unsigned i = 0; i < size1; ++i)
	{
		_pol1_2d.append_vertex (Vector<Value> (_pol1[i][x], _pol1[i][y]) );
	}

	for (unsigned i = 0; i < size2; ++i)
	{
		_pol2_2d.append_vertex (Vector<Value> (_pol2[i][x], _pol2[i][y]) );
	}

	for (unsigned  i = 0, j = size1 - 1; i < size1; j = i++)
	{
		for (unsigned  m = 0, n = size2 - 1; m < size2; n = m++)
		{
			if (predicate_seg_seg_2d(Segment<Value> (_pol1_2d[i], _pol1_2d[j]), Segment<Value> (_pol2_2d[m], _pol2_2d[n])) ) 
			{
				return true; 
			}
		}
	}

	if (is_in_polgyon_2d(_pol1_2d, _pol2_2d[0]) && is_in_polgyon_2d(_pol1_2d, _pol2_2d[1]) && is_in_polgyon_2d(_pol1_2d, _pol2_2d[0]))
	{
		return true; 
	}
	else if (is_in_polgyon_2d(_pol2_2d, _pol1_2d[0]) && is_in_polgyon_2d(_pol2_2d, _pol1_2d[0]) && is_in_polgyon_2d(_pol2_2d, _pol1_2d[0]))
	{
		return true; 
	}
	else 
	{
		for (unsigned i = 0; i < _pol2_2d.size(); ++i)
		{
			if (is_in_polgyon_2d(_pol1_2d, _pol2_2d[i])) 
			{
				return true; 
			}
		}
	}
	return false; 
	
	//return true; 
}
template <typename Value>
bool predicate_polygon_polygon_convex (const Polygon<Value> & _pol1, const Polygon<Value> & _pol2)
{
	Vector<Value> normal1 = _pol1.calculate_normal();
	Vector<Value> normal2 = _pol2.calculate_normal();

	unsigned size1 = _pol1.size();
	unsigned size2 = _pol2.size(); 
	unsigned u0, u1, u2, u3;
	u0 =  u1 =  u2 = u3 = size1; 

	unsigned coplanar1 = 0;

	unsigned i = 0, j = size1 - 1; 
	for (; i < size1; j = i++)
	{
		Value d0 = (_pol1[j] - _pol2[0]) * normal2 ; // Predicate
		Value d1 = (_pol1[i] - _pol2[0]) * normal2 ; // Predicate
		if (abs(d1)  < Value(tiny_value)) {coplanar1 ++; }
		if (d0 * d1 <= Value(tiny_value))
		{
			if (d0 >= Value(tiny_value) && d1 < -Value(tiny_value)) { u0 = j ; u1 = i; }
			else if (d0 > Value(tiny_value) && d1 <= -Value(tiny_value)) { u0 = j ; u1 = i; }
			else if (d1 >= Value(tiny_value) && d0 < -Value(tiny_value)){ u3 = i ; u2 = j; }
			else if (d1 > Value(tiny_value) && d0 <= -Value(tiny_value)){ u3 = i ; u2 = j; }
		}
/*
  if (d0 * d1 <= Value(0))
  {
  if (d0 >= Value(0) && d1 < Value(0)) { u0 = j ; u1 = i; }
  else if (d0 > Value(0) && d1 <= Value(0)) { u0 = j ; u1 = i; }
  else if (d1 >= Value(0) && d0 < Value(0)){ u3 = i ; u2 = j; }
  else if (d1 > Value(0) && d0 <= Value(0)){ u3 = i ; u2 = j; }
			
  }
*/
	}
	//for (unsigned i = 0; i < coplanar_id.size(); ++i) std::cout<<coplanar_id[i]<<' ';
	//std::cout<<std::endl;
	if (coplanar1 >= 3) 
	{	
		return (predicate_polygon_polygon_convex_coplanar(_pol1, _pol2));
	}
/*
  else
  if (coplanar == 2)
  {
  return (predicate_polygon_convex_segment_coplanar(_pol2, Segment<Value> (_pol1[coplanar_id[0]], _pol1[coplanar_id[1]] ) ));
  //throw ("An edge on the face. ");
  }
*/
	if ( u0 == size1 || u2 == size1 ) return false;  
	
	unsigned v0, v1, v2, v3 ;
	v0 = v1 = v2 = v3 = size2; 
	unsigned coplanar2 = 0; 
	for (i = 0, j = size2 - 1; i < size2; j = i++)
	{
		Value d0 = ( _pol2[j] - _pol1[0] ) * normal1 ; // Predicate
		Value d1 = ( _pol2[i] - _pol1[0] ) * normal1 ; // Predicate
		if (abs(d1)  <Value(tiny_value)) {coplanar2 ++; }
		if (d0 * d1 <= Value(tiny_value))
		{
			if (d0 >= Value(tiny_value) && d1 < -Value(tiny_value)) { v0 = j ; v1 = i; }
			else if (d0 > Value(tiny_value) && d1 <= -Value(tiny_value)) { v0 = j ; v1 = i; }
			else if (d1 >= Value(tiny_value) && d0 < -Value(tiny_value)){ v3 = i ; v2 = j; }
			else if (d1 > Value(tiny_value) && d0 <= -Value(tiny_value)){ v3 = i ; v2 = j; }
		}
/*
  if (d0 * d1 <= Value(0))
  {
  if (d0 >= Value(0) && d1 < Value(0)) { v0 = j ; v1 = i; }
  else if (d0 > Value(0) && d1 <= Value(0)) { v0 = j ; v1 = i; }
  else if (d1 >= Value(0) && d0 < Value(0)){ v3 = i ; v2 = j; }
  else if (d1 > Value(0) && d0 <= Value(0)){ v3 = i ; v2 = j; }
  }
*/
	}
	if (v0 == size2 || v2 == size2 ) return false; 

	//[u0, u1, v0, v1] <=0 Predicate
	Value d0 = orient_3d(_pol1[u0], _pol1[u1], _pol2[v0], _pol2[v1]);
	//[u2, u3, v3, v2] <=0 Predicate
	Value d1 = orient_3d(_pol1[u2], _pol1[u3], _pol2[v3], _pol2[v2]);

//	if (coplanar1 !=2 && coplanar2 == 2)
	return (d0 <= 0 && d1 <= 0);
//	else return false; 

}


template <typename value_>
struct predicate_poly_poly_impl
{
	typedef value_ value_type; 
	typedef Polygon<value_type> polygon_type;
	static bool predicate (const polygon_type & _pol1, const polygon_type & _pol2) 
	{
		return predicate_polygon_polygon_convex(_pol1, _pol2);
	}
	//predicate_polygon_polygon_convex
};

/*
template <>
struct predicate_poly_poly_impl<double>
{

	typedef long double value_type; 
	typedef Polygon<value_type> long_polygon_type;
	typedef Polygon<double> polygon_type;

	static  void convert (const polygon_type & p, long_polygon_type & long_p)
	{
		for (unsigned i = 0; i < p.size(); ++i) 
			long_p.append_vertex(Vector <long double> (p[i][0], p[i][1], p[i][2]));

	}
	static bool predicate (const polygon_type & _pol1, const polygon_type & _pol2) 
	{
		long_polygon_type lp1 , lp2; 
		convert (_pol1, lp1) ;
		convert (_pol2, lp2) ; 
		return predicate_polygon_polygon_convex(lp1, lp2);
	}
	//predicate_polygon_polygon_convex
};
*/
/**
 * This function handle the strictly convex polygon - polygon. 
 - The general method is described in http://home.arcor.de/philippe.guigue/Sample_codes/poly_poly_intersection.ps.gz
 */
template <typename Value>
inline bool predicate_polygon_polygon (const Polygon <Value> & _pol1, const Polygon <Value> & _pol2)
{
	return predicate_poly_poly_impl<Value>::predicate(_pol1, _pol2) ;
}



//////////////////////////////////////////////////////////////////////////////////
// HERE ARE THE PREDICATORS                                  /////////////////////
// Acutally, they are not truly predicates.                  /////////////////////
//////////////////////////////////////////////////////////////////////////////////


template <typename Value>
bool predicate_segment_box (const Segment <Value> & _segment, const Box <Value> & box)
{
	// Check if one end is in the box.
	if (box.isInside (_segment.p0) || box.isInside(_segment.p1)) return true; 
	
	
	Value fAWdU[3], fADdU[3], fAWxDdU[3], fRhs;
	Vector<Value> kSDir = Value(0.5) * (_segment.p1 - _segment.p0);
	Vector<Value> kSCen = _segment.p0 + kSDir;

	Vector<Value> kDiff = kSCen - box.center;

	fAWdU[0] = abs(kSDir[0]);
	fADdU[0] = abs(kDiff[0]);
	fRhs = box.extent[0] + fAWdU[0];
	if (fADdU[0] > fRhs)
		return false;

	fAWdU[1] = abs(kSDir[1]);
	fADdU[1] = abs(kDiff[1]);
	fRhs = box.extent[1] + fAWdU[1];
	if (fADdU[1] > fRhs)
		return false;

	fAWdU[2] = abs(kSDir[2]);
	fADdU[2] = abs(kDiff[2]);
	fRhs = box.extent[2] + fAWdU[2];
	if (fADdU[2] > fRhs)
		return false;

	Vector<Value> kWxD = kSDir ^ kDiff;

	fAWxDdU[0] = abs(kWxD[0]);
	fRhs = box.extent[1] * fAWdU[2] + box.extent[2] * fAWdU[1];
	if (fAWxDdU[0] > fRhs)
		return false;

	fAWxDdU[1] = abs(kWxD[1]);
	fRhs = box.extent[0] * fAWdU[2] + box.extent[2] * fAWdU[0];
	if (fAWxDdU[1] > fRhs)
		return false;

	fAWxDdU[2] = abs(kWxD[2]);
	fRhs = box.extent[0] * fAWdU[1] + box.extent[1] * fAWdU[0];
	if (fAWxDdU[2] > fRhs)
		return false;

    return true;

}

template <typename Value>
bool predicate_polygon_segment (const Polygon <Value> & _pol, const Segment <Value> & _segment)
{
	IntersectResult<Value> _intr; 
	return intersect (_pol, _segment, _intr) ;
}




/**
 * Predicate the intersection between Polygon and a Box. 
 - Check if any vertex of polygon is inside the box. 
 * @link http://jgt.akpeters.com/papers/AkenineMoller01/tribox.html Triangle overlap with box @endlink
 */
#define FINDMINMAX(x0,x1,x2,min,max)			\
	min = max = x0;								\
	if(x1<min) min=x1;							\
	if(x1>max) max=x1;							\
	if(x2<min) min=x2;							\
	if(x2>max) max=x2;

template <typename Value>
int planeBoxOverlap(const Vector<Value> &  normal, const Value & d, const Vector<Value> & maxbox)
{
	Vector<Value> vmin(3),vmax(3);
	for(int q=0; q <3 ; ++q)
	{
		if(normal[q]>0.0f)
		{
			vmin[q]=-maxbox[q];
			vmax[q]=maxbox[q];
		}
		else
		{
			vmin[q]=maxbox[q];
			vmax[q]=-maxbox[q];
		}
	}
//	if(normal * vmin + d > 0.0f) return 0;
//	if(normal * vmax + d >=0.0f) return 1;

	Value test1 = normal * vmin + d;
	Value test2 = normal * vmax + d;
	if(test1 > tiny_value) return 0;
	if(abs(test2) <= tiny_value || test2 >=Value(0.0) ) return 1;
	
  
	return 0;
}


// ======================== X-tests ========================
#define AXISTEST_X01(a, b, fa, fb)						\
	p0 = a*v0[1] - b*v0[2];								\
	p2 = a*v2[1] - b*v2[2];								\
	if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;}	\
	rad = fa * boxhalfsize[1] + fb * boxhalfsize[2];	\
	if(min>rad + tiny_value || max<-rad - tiny_value) return 0;

#define AXISTEST_X2(a, b, fa, fb)						\
	p0 = a*v0[1] - b*v0[2];								\
	p1 = a*v1[1] - b*v1[2];								\
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;}	\
	rad = fa * boxhalfsize[1] + fb * boxhalfsize[2];	\
	if(min>rad + tiny_value || max<-rad - tiny_value) return 0;

// ======================== Y-tests ========================
#define AXISTEST_Y02(a, b, fa, fb)						\
	p0 = -a*v0[0] + b*v0[2];							\
	p2 = -a*v2[0] + b*v2[2];							\
	if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[2];	\
	if(min>rad + tiny_value || max<-rad - tiny_value) return 0;

#define AXISTEST_Y1(a, b, fa, fb)						\
	p0 = -a*v0[0] + b*v0[2];							\
	p1 = -a*v1[0] + b*v1[2];							\
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[2];	\
	if(min>rad + tiny_value || max<-rad - tiny_value) return 0;

// ======================== Z-tests ========================

#define AXISTEST_Z12(a, b, fa, fb)						\
	p1 = a*v1[0] - b*v1[1];								\
	p2 = a*v2[0] - b*v2[1];								\
	if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[1];	\
	if(min>rad + tiny_value|| max<-rad - tiny_value) return 0;

#define AXISTEST_Z0(a, b, fa, fb)						\
	p0 = a*v0[0] - b*v0[1];								\
	p1 = a*v1[0] - b*v1[1];								\
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[1];	\
	if(min>rad + tiny_value|| max<-rad - tiny_value) return 0;

/*
// ======================== X-tests ========================
#define AXISTEST_X01(a, b, fa, fb)						\
	p0 = a*v0[1] - b*v0[2];								\
	p2 = a*v2[1] - b*v2[2];								\
	if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;}	\
	rad = fa * boxhalfsize[1] + fb * boxhalfsize[2];	\
	if(min>rad  || max<-rad ) return 0;

#define AXISTEST_X2(a, b, fa, fb)						\
	p0 = a*v0[1] - b*v0[2];								\
	p1 = a*v1[1] - b*v1[2];								\
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;}	\
	rad = fa * boxhalfsize[1] + fb * boxhalfsize[2];	\
	if(min>rad  || max<-rad ) return 0;

// ======================== Y-tests ========================
#define AXISTEST_Y02(a, b, fa, fb)						\
	p0 = -a*v0[0] + b*v0[2];							\
	p2 = -a*v2[0] + b*v2[2];							\
	if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[2];	\
	if(min>rad || max<-rad ) return 0;

#define AXISTEST_Y1(a, b, fa, fb)						\
	p0 = -a*v0[0] + b*v0[2];							\
	p1 = -a*v1[0] + b*v1[2];							\
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[2];	\
	if(min>rad || max<-rad ) return 0;

// ======================== Z-tests ========================

#define AXISTEST_Z12(a, b, fa, fb)						\
	p1 = a*v1[0] - b*v1[1];								\
	p2 = a*v2[0] - b*v2[1];								\
	if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[1];	\
	if(min>rad || max<-rad ) return 0;

#define AXISTEST_Z0(a, b, fa, fb)						\
	p0 = a*v0[0] - b*v0[1];								\
	p1 = a*v1[0] - b*v1[1];								\
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;}	\
	rad = fa * boxhalfsize[0] + fb * boxhalfsize[1];	\
	if(min>rad || max<-rad ) return 0;
*/

template <typename Value>
bool triBoxOverlap(const Vector<Value> & boxcenter, const Vector<Value> & boxhalfsize,const Polygon <Value> & triverts)
{

	/*    use separating axis theorem to test overlap between triangle and box */
	/*    need to test for overlap in these directions: */
	/*    1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
	/*       we do not even need to test these) */
	/*    2) normal of the triangle */
	/*    3) crossproduct(edge from tri, {x,y,z}-directin) */
	/*       this gives 3x3=9 more tests */
	Vector<Value> v0, v1, v2 ;
	Vector<Value> axis(3);
	Value min,max,d,p0,p1,p2,rad,fex,fey,fez;  
	Vector<Value> normal, e0, e1, e2;

	/* This is the fastest branch on Sun */
	/* move everything so that the boxcenter is in (0,0,0) */
	v0 = triverts[0] - boxcenter; 
	v1 = triverts[1] - boxcenter; 
	v2 = triverts[2] - boxcenter; 

	/* compute triangle edges */
	e0 = v1 - v0;
	e1 = v2 - v1;
	e2 = v0 - v2;


	/* Bullet 3:  */
	/*  test the 9 tests first (this was faster) */
	fex = fabs(e0[0]);
	fey = fabs(e0[1]);
	fez = fabs(e0[2]);
	AXISTEST_X01(e0[2], e0[1], fez, fey);
	AXISTEST_Y02(e0[2], e0[0], fez, fex);
	AXISTEST_Z12(e0[1], e0[0], fey, fex);

	fex = fabs(e1[0]);
	fey = fabs(e1[1]);
	fez = fabs(e1[2]);
	AXISTEST_X01(e1[2], e1[1], fez, fey);
	AXISTEST_Y02(e1[2], e1[0], fez, fex);
	AXISTEST_Z0 (e1[1], e1[0], fey, fex);

	fex = fabs(e2[0]);
	fey = fabs(e2[1]);
	fez = fabs(e2[2]);
	AXISTEST_X2 (e2[2], e2[1], fez, fey);
	AXISTEST_Y1 (e2[2], e2[0], fez, fex);
	AXISTEST_Z12(e2[1], e2[0], fey, fex);

	/* Bullet 1: */
	/*  first test overlap in the {x,y,z}-directions */
	/*  find min, max of the triangle each direction, and test for overlap in */
	/*  that direction -- this is equivalent to testing a minimal AABB around */
	/*  the triangle against the AABB */

	/* test in X-direction */
	FINDMINMAX(v0[0],v1[0],v2[0],min,max);
	if(min>boxhalfsize[0] + tiny_value|| max<-boxhalfsize[0] - tiny_value) return false;

	/* test in Y-direction */
	FINDMINMAX(v0[1],v1[1],v2[1],min,max);
	if(min>boxhalfsize[1] + tiny_value|| max<-boxhalfsize[1] - tiny_value) return false;

	/* test in Z-direction */
	FINDMINMAX(v0[2],v1[2],v2[2],min,max);
	if(min>boxhalfsize[2] + tiny_value || max<-boxhalfsize[2] - tiny_value) return false;

	/* Bullet 2: */
	/*  test if the box intersects the plane of the triangle */
	/*  compute plane equation of triangle: normal*x+d=0 */
	normal = e0 ^ e1;
	d = normal * v0 ;
	if(!planeBoxOverlap(normal,d,boxhalfsize)) return false;

	return true;   /* box and triangle overlaps */
}

template <typename Value>
bool predicate_polygon_box (const Polygon <Value> & _pol, const Box <Value> & _box)
{
	// Check if any vertex of polygon is inside the box. 
	// The idea is we split the convex polygon to be triangle and do the test.
	// If the polygon is not convex. WE will do triangulation ? Who knows. 
	if (_pol.size() !=3 ) throw ("Polygon box implementation is pending. ");
	return  triBoxOverlap(_box.center, _box.extent, _pol) ;
}



/**
 * This function now handle triangle intersects triangle.
 */
template <typename Value>
bool predicate_tri_tri (const Polygon <Value> & _pol1, const Polygon <Value> & _pol2)
{
	/*
	double p1[3], q1[3], r1[3];
	double p2[3], q2[3], r2[3];

	p1[0] = _pol1[0][0]; p1[1] = _pol1[0][1]; p1[2] = _pol1[0][2];
	q1[0] = _pol1[1][0]; q1[1] = _pol1[1][1]; q1[2] = _pol1[1][2];
	r1[0] = _pol1[2][0]; r1[1] = _pol1[2][1]; r1[2] = _pol1[2][2];
	
	p2[0] = _pol2[0][0]; p2[1] = _pol2[0][1]; p2[2] = _pol2[0][2];
	q2[0] = _pol2[1][0]; q2[1] = _pol2[1][1]; q2[2] = _pol2[1][2];
	r2[0] = _pol2[2][0]; r2[1] = _pol2[2][1]; r2[2] = _pol2[2][2];
	
	return tri_tri_overlap_test_3d(p1, q1, r1, p2, q2, r2);
	*/
	return tri_tri_overlap_test_3d(_pol1[0], _pol1[1], _pol1[2], _pol2[0], _pol2[1], _pol2[2]);

}


/**
 * Decide along which axis to do 2D map. 
 * @return 0-x, 1-y, 2-z; 
 */
template <typename Value>
unsigned select_map_axis(const Vector<Value> & normal,  unsigned & x, unsigned & y) 
{
	Value ax, ay, az ;
	unsigned coord ; // coord to be ignored. 0-x, 1-y, 2-z
		
	ax = abs(normal[0]) ;
	ay = abs(normal[1]) ;
	az = abs(normal[2]) ;
		
	coord = 2; 
	if (ax > ay )
	{
		if (ax > az) coord = 0;
	}
	else
	{
		if (ay > az) coord = 1 ;
	}

	x = (coord + 1) % 3; 
	y = (coord + 2) % 3;

	return coord; 
}

/**
 * Map 3D points to 2D points. 
 * @pre {select_map_axis}
 */
template <typename Value>
void map_3D_point_to_2D_point (unsigned x, unsigned y, const Vector<Value> & pt3d, Vector<Value> & pt2d)
{
}



/**
 * Map 2D points to 3D points. 
 * @pre {select_map_axis}
 */
template <typename Value>
void map_2D_point_to_3D_point (unsigned x, unsigned y, const Plane<Value> & plane, const Vector<Value> & pt2d, Vector<Value> & pt3d)
{
	Value a, b, c, d; 
	plane.get_parameter (a,b,c,d); 
	if (x == 0 && y == 1)
	{
		pt3d = Vector<Value> (pt2d[0], pt2d[1], ( -d - a * pt2d[0] - b * pt2d[1] ) / c);
	} else
	if (x == 2 && y == 0)
	{
		pt3d = Vector<Value> (pt2d[1], ( -d - a * pt2d[1] - c * pt2d[0] ) / b, pt2d[0]);
	} else
	if (x == 1 && y == 2)
	{
		pt3d = Vector<Value> ( ( -d - b * pt2d[0] - c * pt2d[1] ) / a, pt2d[0], pt2d[1]);
	}
}

/**
 * predicate the ray and the triangle.
 * http://jgt.akpeters.com/papers/Chirkov05/C2005.zip
 * @pre The direction of the ray must be normalized. 
 */
#define EPSILON 1e-14 

#define CROSS(dest,v1,v2) \
          dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
          dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
          dest[2]=v1[0]*v2[1]-v1[1]*v2[0];

#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

#define SUB(dest,v1,v2)\
          dest[0]=v1[0]-v2[0]; \
          dest[1]=v1[1]-v2[1]; \
          dest[2]=v1[2]-v2[2]; 
template <typename Value>
bool _intersect_ray_tri(Value orig[3], Value dir[3],
					  Value vert0[3], Value vert1[3], Value vert2[3],
					  Value *t, Value *u, Value *v)
{

        double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
        double det,inv_det;

        /* find vectors for two edges sharing rt->v0 */
        SUB(edge1, vert1, vert0);
		SUB(edge2, vert2, vert0);

        /* begin calculating determinant - also used to calculate U parameter */
        CROSS(pvec, dir, edge2);

        /* if determinant is near zero, ray lies in plane of triangle */
        det = DOT(edge1, pvec);

        /* calculate distance from rt->v0 to ray origin */
        SUB(tvec, orig, vert0);
        inv_det = 1.0 / det;

        if (det > tiny_value)
        {
                /* calculate U parameter and test bounds */
                *u = DOT(tvec, pvec);
                if (*u < 0.0 || *u > det)
                        return 0;

                /* prepare to test V parameter */
                CROSS(qvec, tvec, edge1);

                /* calculate V parameter and test bounds */
                *v = DOT(dir, qvec);
                if (*v < 0.0 || *u + *v > det)
                        return 0;

        }
        else if(det < - tiny_value)
        {
                /* calculate U parameter and test bounds */
                *u = DOT(tvec, pvec);
                if (*u > 0.0 || *u < det)
                        return 0;

                /* prepare to test V parameter */
                CROSS(qvec, tvec, edge1);

                /* calculate V parameter and test bounds */
                *v = DOT(dir, qvec) ;
                if (*v > 0.0 || *u + *v < det)
                        return 0;
        }
        else 
		{
			//throw ("ray is paralle to the plane of the triangle");
			return 0;  /* ray is parallell to the plane of the triangle */
		}

        /* calculate t, ray intersects triangle */
        *t = DOT(edge2, qvec) * inv_det;
        //if(*t<0.0 || *t>1.0)      return 0; // BUG. FIXED. 
		if (*t < - tiny_value ) return 0; 
/*
		std::cout<<"u "<<*u<<" v "<<*v<< " t "<<*t<<std::endl;
		*u = std::abs((*u) * det);
		*v = std::abs((*v) * det);
		*/
        return 1;

}

/**
 * predicate the ray and the triangle 
 */
template <typename Value>
bool predicate_ray_tri (const Ray<Value> & _ray, const Polygon<Value> & _pol)
{
	
	if (_pol.size() != 3) throw ("Ray - triangle perdicate is not called with a triangle. ");
	Value orig[3], dir[3];
	Value vert0[3], vert1[3],  vert2[3]; 
	Value t, u, v ;
	Vector<Value> p, d ;
	p = _ray.get_point();
	d = _ray.get_direction ();

	for (int i = 0; i < 3; ++i)
	{
		orig[i] = p[i];
		dir[i]  = d[i];
		vert0[i] = _pol[0][i];
		vert1[i] = _pol[1][i];
		vert2[i] = _pol[2][i];
	}
	return _intersect_ray_tri(orig, dir, vert0, vert1, vert2, & t, & u, &v);
	
}

/**
 * predicate the ray and the triangle 
 */
template <typename Value>
bool predicate_polygon_ray (const Polygon<Value> & _pol, const Ray<Value> & _ray)
{
	if (_pol.size() > 3) throw ("Ray- polygon perdicate is not implemented. ");
	return predicate_ray_tri(_ray, _pol);
}

/**
 * Ray-box intersection using IEEE numerical properties to ensure that the
 * test is both robust and efficient, as described in:
 *
 -     Amy Williams, Steve Barrus, R. Keith Morley, and Peter Shirley
 *      "An Efficient and Robust Ray-Box Intersection Algorithm"
 *      Journal of graphics tools, 10(1):49-54, 2005
 *
 *
 - Note. If the origin of the ray falls on the box face. We also think they do not intersect. 
 - The Value type must support 1.0 / 0.0. Core library doesn't support this. 
 */
template <typename Value>
bool predicate_box_ray (const Box<Value> & _box, const Ray<Value> & _ray)
{
	/*
	const Value t = Value(1e10);
	Vector<Value> origin = _ray.get_point();
	Vector<Value> dir = _ray.get_direction();

	Segment<Value> seg (origin, origin + t * dir);
	return predicate_segment_box(seg, _box);
	*/
	
	const Value t0 = Value(0);     // This value can be negative. 
	const Value t1 = Value(1e10);  // Change this value to extend the lenght of ray.
	Value tmin, tmax, tymin, tymax, tzmin, tzmax;
	int sign[3];
	Vector<Value> inv_dir (3);
	Vector<Value> origin = _ray.get_point();
	Vector<Value> dir = _ray.get_direction();

	Vector<Value> parameters[2];
	parameters[0] = _box.getMin();
	parameters[1] = _box.getMax();


	inv_dir[0] = Value(1.0)/ dir[0];
    inv_dir[1] = Value(1.0)/ dir[1];
	inv_dir[2] = Value(1.0)/ dir[2];


	sign[0] = (inv_dir[0] < 0 );
	sign[1] = (inv_dir[1] < 0 );
	sign[2] = (inv_dir[2] < 0 );


	tmin = (parameters[sign[0]][0] -  origin[0]) * inv_dir[0];
	tmax = (parameters[1- sign[0]][0] - origin[0]) * inv_dir[0];
	tymin = (parameters[sign[1]][1] - origin[1]) * inv_dir[1];
	tymax = (parameters[1-sign[1]][1] - origin[1]) * inv_dir[1];
	if ( (tmin > tymax) || (tymin > tmax) ) 
		return false;
	if (tymin > tmin)
		tmin = tymin;
	if (tymax < tmax)
		tmax = tymax;
	tzmin = (parameters[sign[2]][2] - origin[2]) * inv_dir[2];
	tzmax = (parameters[1-sign[2]][2] - origin[2]) * inv_dir[2];
	if ( (tmin > tzmax) || (tzmin > tmax) ) 
		return false;
	if (tzmin > tmin)
		tmin = tzmin;
	if (tzmax < tmax)
		tmax = tzmax;
	//return ( (tmin < t1) && (tmax > t0) ); // The original cannot falls on the box. 
	//return ( (tmin < t1) && (tmax >= t0) ); // The original can falls on the box. 
	return ( (tmin < t1) && (tmax >= t0 || abs(tmax - t0) <= tiny_value) ); // The original can falls on the box. 
}

#endif
